#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <memory>
#include <vector>
#include <cmath>

#include "fishbot_setparam/amcl_setparam.h"
#include "fishbot_setparam/nav2param_manager.h"
#include "std_msgs/msg/int16.hpp"

using namespace std::chrono_literals;
using namespace std::placeholders;

struct GoalPoint
{
  double x;
  double y;
  double w;
  double z;
};

class GoalSender : public rclcpp::Node
{
public:
  GoalSender() : Node("custom_goal_sender"), goal_index_(0), total_distance_(0.0), max_distance_remaining_(0.0)
  {
    // 初始化目标点列表
    goals_ = {
        {1.75, -0.56, 1.0, 0.0},
        {-1.45, -0.54, 0.0,1.0}};

    // 创建动作客户端
    action_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
        this,
        "navigate_to_pose");
    gray_state_sub_ = this->create_subscription<std_msgs::msg::Int16>(
          "/global_costmap/gray_state", 10, std::bind(&GoalSender::grayStateCallBack, this, std::placeholders::_1));
    amcl_param_updater_node_ = std::make_shared<AmclParamUpdater>();
    param_manager_node_ = std::make_shared<rclcpp::Node>("nav2_param_updater");
    param_manager_ = std::make_shared<Nav2ParamManager>(param_manager_node_);
    // 发送第一个目标点
    send_next_goal();
  }

private:
  void send_next_goal()
  {
    if (!action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
      return;
    }

    auto current_goal = goals_[goal_index_];
    RCLCPP_INFO(this->get_logger(), "Sending goal %ld/%d",
                goal_index_ + 1, static_cast<int>(goals_.size()));

    auto goal_msg = nav2_msgs::action::NavigateToPose::Goal();
    goal_msg.pose.header.frame_id = "map";
    goal_msg.pose.header.stamp = this->now();
    goal_msg.pose.pose.position.x = current_goal.x;
    goal_msg.pose.pose.position.y = current_goal.y;
    goal_msg.pose.pose.orientation.w = current_goal.w;
    goal_msg.pose.pose.orientation.z = current_goal.z;
    goal_msg.pose.pose.orientation.x = 0.0;
    goal_msg.pose.pose.orientation.y = 0.0;

    auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
    send_goal_options.goal_response_callback =
        std::bind(&GoalSender::goal_response_callback, this, _1);
    send_goal_options.result_callback =
        std::bind(&GoalSender::result_callback, this, _1);
    send_goal_options.feedback_callback =
        std::bind(&GoalSender::feedback_callback, this, _1, _2);

    action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  void goal_response_callback(
      const rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr &goal_handle)
  {
    if (!goal_handle)
    {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    }
    else
    {
      RCLCPP_INFO(this->get_logger(), "Goal accepted by server");
    }
  }

  void result_callback(
      const rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::WrappedResult &result)
  {
    switch (result.code)
    {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_INFO(this->get_logger(), "Navigation completed!");
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "Navigation was aborted");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "Navigation was canceled");
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "Unknown result code");
      return;
    }

    // 更新总距离并准备下一个目标点
    total_distance_ += max_distance_remaining_;
    RCLCPP_INFO(this->get_logger(), "Total distance: %.2f meters", total_distance_);

    goal_index_ = (goal_index_ + 1) % goals_.size();
    // if (goal_index_ == 1)
    //   amcl_param_updater_node_->update();
    max_distance_remaining_ = 0.0;
    send_next_goal();
  }

  void feedback_callback(
      rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr,
      const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> feedback)
  {
    if (feedback->distance_remaining > max_distance_remaining_)
    {
      max_distance_remaining_ = feedback->distance_remaining;
    }
  }

  // 成员变量
  rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr action_client_;
  std::vector<GoalPoint> goals_;
  size_t goal_index_;
  double total_distance_;
  double max_distance_remaining_;
  rclcpp::Node::SharedPtr param_manager_node_;
  std::shared_ptr<Nav2ParamManager> param_manager_;

  std::shared_ptr<AmclParamUpdater> amcl_param_updater_node_;


  rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr gray_state_sub_;

  void grayStateCallBack(const std_msgs::msg::Int16::SharedPtr msg);
};